#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <cstring>
#include <cctype>
#include <iostream>
#include <fstream>

using namespace std;

#include "Contour.h"
#include "../ImprovedMath.h"
#include "../Matrix.h"
#include "Polynomial.h"
#include "LeastSquares.h"
#include "GeometricData.h"
#include "../random.h"

#include "../LogfileEntry.h"

bool NormalizeVector( Matrix& Input )
{
 extern double _EPS;
 
 double norm = Input.IntPnorm(2);
 if( norm < _EPS )
 { return false; }

 for( int i=0 ; i < Input.TellRows() ; i++ )
 {
  for( int j=0; j < Input.TellCols() ; j++ )
  {
   *Input(i,j) = *Input(i,j) / norm;  
  }
 }

 return true;
}

DataPoint::DataPoint()
{
 x = 0;
 y = 0;
 pPreviousDataPoint = NULL;
 pNextDataPoint = NULL;
}

DataPoint::~DataPoint()
{
 return;
}

Contour::Contour()
{
 pFirstDataPoint = NULL;
 Value = 0.0;
 
 pPreviousContour = NULL;
 pNextContour = NULL;
}

Contour::~Contour()
{
 UncloseContour(); 

 if( pPreviousContour )
 { pPreviousContour->pNextContour = pNextContour; }
 if( pNextContour )
 { pNextContour->pPreviousContour = pPreviousContour; }

 DataPoint* pCurrentDataPoint = pFirstDataPoint;
 if( pCurrentDataPoint )
 {
  while( pCurrentDataPoint->pNextDataPoint )
  {
   pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint;
   delete pCurrentDataPoint->pPreviousDataPoint;
  }
  delete pCurrentDataPoint;
 }
}

bool Contour::InitializeFromMatrix( Matrix& Xin, Matrix& Yin, Matrix& Input , double value )
{
 extern double _EPS;

 DataPoint* pCurrentDataPoint = pFirstDataPoint;
 Value = value;
 
 double dxIn = *Xin(1)-*Xin(0);
 double dyIn = *Yin(1)-*Yin(0);
 
 int count = 0;
 
 extern char* StringBuffer;
 extern int StringBufferLength;
 extern char* LogFile;
 
 for( int i=0 ; i < Input.TellRows() ; i++ )
 {
  for( int j=0; j < Input.TellCols()-1 ; j++ )
  {
   bool found = false;
   
   double val1 = *Input(i,j);
   double val2 = *Input(i,j+1);
   if( val1-value <= _EPS && val2-value > _EPS )
   { found = true; }
   if( val1-value > _EPS && val2-value <= _EPS )
   { found = true; }
   
   if( found )
   {
    count++;
    double theta = fabs(val1)/( fabs(val1)+fabs(val2) );
    DataPoint* pNewDataPoint;
    pNewDataPoint = new DataPoint;
    
    pNewDataPoint->x = *Xin(i);
    pNewDataPoint->y = *Yin(j) + theta*dyIn;
	pNewDataPoint->pPreviousDataPoint = pCurrentDataPoint;
	pNewDataPoint->pNextDataPoint = NULL;
	
	if( pCurrentDataPoint )
	{ pCurrentDataPoint->pNextDataPoint = pNewDataPoint; }
	
	pCurrentDataPoint = pNewDataPoint; 
    if( !pFirstDataPoint )
    { pFirstDataPoint = pNewDataPoint; }	
   }
  
  }
 }

 for( int i=0 ; i < Input.TellRows()-1 ; i++ )
 {
  for( int j=0; j < Input.TellCols() ; j++ )
  {
   bool found = false;
   
   double val1 = *Input(i,j);
   double val2 = *Input(i+1,j);
   if( val1-value <= _EPS && val2-value > _EPS )
   { found = true; }
   if( val1-value > _EPS && val2-value <= _EPS )
   { found = true; }
   
   if( found )
   {
    count++;
    double theta = fabs(val1)/( fabs(val1)+fabs(val2) );
    DataPoint* pNewDataPoint;
    pNewDataPoint = new DataPoint;
    
    pNewDataPoint->x = *Xin(i)+theta*dxIn;
    pNewDataPoint->y = *Yin(j);
	pNewDataPoint->pPreviousDataPoint = pCurrentDataPoint;
	pNewDataPoint->pNextDataPoint = NULL;
	
	if( pCurrentDataPoint )
	{ pCurrentDataPoint->pNextDataPoint = pNewDataPoint; }

	pCurrentDataPoint = pNewDataPoint; 
    if( !pFirstDataPoint )
    { pFirstDataPoint = pNewDataPoint; }	
   }
  
  }
 }
  
 if( count > 0 )
 {
  Output( "xpts.dat" , "ypts.dat" );
  CompletelyOrganizeIntoClosedContours();
  PositivelyOrientContour(Xin,Yin,Input);
 }
 else
 {
  StartStringBuffer( "Warning: No points found on the desired contour!\n" );
  cout << StringBuffer << endl;
  MakeLogEntry( LogFile, StringBuffer );
 }

 return true;
}

bool Contour::Output( const char* xfile, const char* yfile )
{
 fstream xstr( xfile, ios::out );
 fstream ystr( yfile, ios::out );
 
 if( !xstr || !ystr )
 { return false; }

 DataPoint* pCurrentDataPoint = pFirstDataPoint; 
 
 while( pCurrentDataPoint )
 {
  xstr << pCurrentDataPoint->x << endl;
  ystr << pCurrentDataPoint->y << endl;
  
  pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint;
  if( pCurrentDataPoint == pFirstDataPoint )
  {
   xstr << pCurrentDataPoint->x << endl;
   ystr << pCurrentDataPoint->y << endl;
   pCurrentDataPoint = NULL; 
  }
 }

 xstr.close();
 ystr.close();
 return true;
}

bool Contour::SortPoints( void )
{
 extern double dx;

 DataPoint* pCurrentDataPoint = pFirstDataPoint;
 
 int count = 0;
 
 while( pCurrentDataPoint && pCurrentDataPoint->pNextDataPoint )
 {
  count++;
  double BestDistance = 9e99;
  
  DataPoint* pBestDataPoint = NULL;   
  
  DataPoint* pCompareDataPoint = pCurrentDataPoint->pNextDataPoint;
   
  while( pCompareDataPoint )
  {
   double distance = square( pCompareDataPoint->x - pCurrentDataPoint->x )
                   + square( pCompareDataPoint->y - pCurrentDataPoint->y );
				     
   if( distance < BestDistance )
   {
    BestDistance = distance;
	pBestDataPoint = pCompareDataPoint;
   }   
   pCompareDataPoint = pCompareDataPoint->pNextDataPoint;  
  }
  
  if( RemovePoint( pBestDataPoint ) )
  { InsertPoint( pCurrentDataPoint , pBestDataPoint ); }

  pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint;
 }
  
 return true;
}

bool Contour::InsertPoint( DataPoint* AfterMe, DataPoint* InsertMe )
{
 if( AfterMe == InsertMe )
 { return false; }
 if( !InsertMe )
 { return false; }
 if( !AfterMe )
 { 
  pFirstDataPoint = InsertMe; 
  InsertMe->pPreviousDataPoint = NULL;
  InsertMe->pNextDataPoint = NULL;
  return true; 
 }
 
 if( AfterMe->pNextDataPoint )
 {
  (AfterMe->pNextDataPoint)->pPreviousDataPoint = InsertMe;
 }
 InsertMe->pNextDataPoint = AfterMe->pNextDataPoint;
 AfterMe->pNextDataPoint = InsertMe;
 InsertMe->pPreviousDataPoint = AfterMe;
  
 return true;
}

bool Contour::RemovePoint( DataPoint* RemoveMe ) // okay on 1-2-2007
{
 if( !RemoveMe )
 { return false; }
 if( pFirstDataPoint == RemoveMe )
 { pFirstDataPoint = RemoveMe->pNextDataPoint; }
 
 if( RemoveMe->pNextDataPoint )
 { (RemoveMe->pNextDataPoint)->pPreviousDataPoint = RemoveMe->pPreviousDataPoint; }
 if( RemoveMe->pPreviousDataPoint )
 { (RemoveMe->pPreviousDataPoint)->pNextDataPoint = RemoveMe->pNextDataPoint; }
 RemoveMe->pNextDataPoint = NULL;
 RemoveMe->pPreviousDataPoint = NULL;
 return true; 
}

bool Contour::CloseContour( void )
{
 if( !pFirstDataPoint )
 { return false; }
 if( pFirstDataPoint->pNextDataPoint == NULL )
 { 
  pFirstDataPoint->pNextDataPoint = pFirstDataPoint; 
  pFirstDataPoint->pPreviousDataPoint = pFirstDataPoint; 
  return true;
 }
 
 DataPoint* pCurrentDataPoint = pFirstDataPoint->pNextDataPoint;
 
 while( pCurrentDataPoint->pNextDataPoint    && 
        pCurrentDataPoint != pFirstDataPoint )
 { pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint; }

 if( pCurrentDataPoint != pFirstDataPoint )
 {
  pCurrentDataPoint->pNextDataPoint = pFirstDataPoint;
  pFirstDataPoint->pPreviousDataPoint = pCurrentDataPoint;
 }
 return true;
}

bool Contour::UncloseContour( void )
{ 
 if( !pFirstDataPoint )
 { return false; }

 DataPoint* pLastDataPoint = pFirstDataPoint->pPreviousDataPoint;
 if( pLastDataPoint )
 {
  pLastDataPoint->pNextDataPoint = NULL;
  pFirstDataPoint->pPreviousDataPoint = NULL;
 }
 return true;
} 

bool Contour::ReverseContour( void )
{
 DataPoint* pLastDataPoint = pFirstDataPoint;

 bool IsClosed = false;
 
 while( pLastDataPoint->pNextDataPoint && 
        pLastDataPoint->pNextDataPoint != pFirstDataPoint )
 {
  pLastDataPoint = pLastDataPoint->pNextDataPoint;
 }
 
 if( pLastDataPoint->pNextDataPoint == pFirstDataPoint )
 {
  IsClosed = true; 
  UncloseContour();
 }

 DataPoint* pNewFirstDataPoint = pLastDataPoint;
 DataPoint* pNewLastDataPoint = pFirstDataPoint;
 
 DataPoint* pCurrentDataPoint = pLastDataPoint;
 while( pCurrentDataPoint )
 {
  DataPoint* pTemp = pCurrentDataPoint->pPreviousDataPoint;
  pCurrentDataPoint->pPreviousDataPoint = pCurrentDataPoint->pNextDataPoint;
  pCurrentDataPoint->pNextDataPoint = pTemp;
  pCurrentDataPoint = pTemp;
 }
 pFirstDataPoint = pLastDataPoint;
 if( IsClosed )
 { CloseContour(); } 
 return true;
}

bool Contour::CleanupContour()
{
 Contour* pCurrentContour = this;
 Contour* pPreviousContourTemp = NULL;
 
 while( pCurrentContour )
 {
  pPreviousContourTemp = pCurrentContour;
  pCurrentContour = pCurrentContour->pNextContour;
  if( pPreviousContourTemp != this )
  { delete pPreviousContourTemp; }
 }
 
 if( !pFirstDataPoint )
 { return true; }
 
 UncloseContour();

 DataPoint* pCurrentDataPoint = this->pFirstDataPoint; 
 while( pCurrentDataPoint->pNextDataPoint )
 {
  pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint;
  delete pCurrentDataPoint->pPreviousDataPoint;
 }
 delete pCurrentDataPoint;
 
 pFirstDataPoint = NULL;
 
 return true; 
}

bool Contour::SeparateContourPieces( void )
{
 extern double dx;
 extern double dy;
// double MaxDistance = square(2)*( square(dx) + square(dy) ); // 1-2-2007
 double MaxDistance = (1 + 1e-8)*( square(dx) + square(dy) );

 Contour* pCurrentContour = this;
 
 DataPoint* pPreviousDataPoint = pCurrentContour->pFirstDataPoint;
 DataPoint* pCurrentDataPoint  = pPreviousDataPoint->pNextDataPoint;
 
 int count = 0;

 while( pCurrentDataPoint )
 {
  count++;
  double distance = square(pPreviousDataPoint->x - pCurrentDataPoint->x)
                  + square(pPreviousDataPoint->y - pCurrentDataPoint->y);  
  if( distance > MaxDistance )
  {
   count = 0;
   Contour* pNewContour;
   pNewContour = new Contour;
   pNewContour->pPreviousContour = pCurrentContour;
   pCurrentContour->pNextContour = pNewContour;   

   pPreviousDataPoint->pNextDataPoint = NULL;
   pCurrentDataPoint->pPreviousDataPoint = NULL;
  
   pCurrentContour = pNewContour;
   pCurrentContour->pFirstDataPoint = pCurrentDataPoint;
  }
  pPreviousDataPoint = pCurrentDataPoint;
  pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint; 
 }
 return true;
}

DataPoint* Contour::FindClosestContourDataPoint( double x, double y )
{
 double BestDistance = 9e99;
 DataPoint* pOutput = NULL;
 
 Contour* pCurrentContour = this;
 while( pCurrentContour )
 {
  DataPoint* pCurrentDataPoint = pCurrentContour->pFirstDataPoint;
  while( pCurrentDataPoint )
  {
   double distance = square( pCurrentDataPoint->x - x ) 
                   + square( pCurrentDataPoint->y - y );
   if( distance < BestDistance )
   {
    BestDistance = distance;
	pOutput = pCurrentDataPoint;
   }   
   pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint;
   if( pCurrentDataPoint == pCurrentContour->pFirstDataPoint )
   { pCurrentDataPoint = NULL; }
  }   
  pCurrentContour = pCurrentContour->pNextContour; 
 }

 return pOutput;
}

bool Contour::CompletelyOrganizeIntoClosedContours( void )
{
 SortPoints();
 SeparateContourPieces();
 RemoveIsolatedContours();
 RemoveBumps();
 
 Contour* pCurrentContour = this;
 while( pCurrentContour )
 {
  pCurrentContour->CloseContour();
  pCurrentContour = pCurrentContour->pNextContour;
 }
 return true;
}

bool Contour::IsPositivelyOriented( Matrix& Xin, Matrix& Yin, Matrix& Input )
{
 extern double _EPS;

 if( pFirstDataPoint->pNextDataPoint == NULL )
 { return true; }
 
 DataPoint* pt0 = pFirstDataPoint;
 DataPoint* pt1 = pFirstDataPoint;
 DataPoint* pt2 = pFirstDataPoint;
 
 // create a decent tangent vector
 
 Matrix Tangent = zeros(1,3);
 if( pt0->pPreviousDataPoint )
 { pt0 = pt0->pPreviousDataPoint; }
 if( pt2->pNextDataPoint )
 { pt2 = pt2->pNextDataPoint; }
 
 *Tangent(0) = pt2->x - pt0->x;
 *Tangent(1) = pt2->y - pt0->y;
 
 double norm = Tangent.Pnorm(2);
 
 // normalize the tangent vector

 NormalizeVector( Tangent );
 
 // find the grid point closest to the first point on the contour
  // find the correct vertical position, or close to it.
 int BestJ = 0;
 double BestDistanceY = 9e99;
 for( int j=0 ; j < Yin.TellCols() ; j++ )
 {
  double distance = square(*Yin(j)-pt1->y);
  if( distance < BestDistanceY )
  { BestJ = j; BestDistanceY = distance; }
 }
 
 // find the correct horizontal position, or close to it. 
 int BestI = 0;
 double BestDistanceX = 9e99;
 for( int i=0 ; i < Xin.TellCols() ; i++ )
 {
  double distance = square(*Xin(i)-pt1->x);
  if( distance < BestDistanceX )
  { BestI = i; BestDistanceX = distance; }
 }

 // select an off-point which is as close to perpendicular to the 
 // tangent vector as possible.
 // If possible, select the point to be "inside" the curve.
 
 Matrix TestVector = zeros(1,3);
 int TestI = BestI;
 int TestJ = BestJ;
 
 if( fabs( *Tangent(1) ) > fabs( *Tangent(0) ) )
 {
  if( TestI > 0 )
  {
   TestI--; 
   if( *Input(TestI,TestJ) >= Value && *Input(TestI+2,TestJ) < Value )
   { TestI += 2; }
  }
  else
  {
   TestI++;
  }
 }
 else
 {
  if( TestJ > 0 )
  {
   TestJ--; 
   if( *Input(TestI,TestJ) >= Value && *Input(TestI,TestJ+2) < Value )
   { TestJ += 2; }
  }
  else
  {
   TestJ++;
  }
 }
 
 *TestVector(0) = *Xin(TestI) - pt1->x;
 *TestVector(1) = *Yin(TestJ) - pt1->y;
 
 NormalizeVector( TestVector );
 
 // test the orientation
 
 Matrix UpVector = CrossProduct( TestVector, Tangent );
 if( *UpVector(2) > _EPS )
 {
  if( *Input(TestI,TestJ) - Value > _EPS )
  { return true; }
  return false; 
 }
 else
 {
  if( *Input(TestI,TestJ) - Value <= _EPS )
  { return true; }
  return false; 
 }
 return true;
}

bool Contour::PositivelyOrientContour( Matrix& Xin, Matrix& Yin, Matrix& Input )
{
 Contour* pCurrentContour = this;
 while( pCurrentContour )
 {
  if( ! pCurrentContour->IsPositivelyOriented(Xin,Yin,Input) )
  { pCurrentContour->ReverseContour(); }
  pCurrentContour = pCurrentContour->pNextContour;
 }
 return true;
}

bool Contour::RemoveIsolatedContours( void )
{
 Contour* pCurrentContour = this;
 while( pCurrentContour )
 {
  int count = 0;
  DataPoint* pCurrentDataPoint = pCurrentContour->pFirstDataPoint;
  while( pCurrentDataPoint->pNextDataPoint && 
         pCurrentDataPoint->pNextDataPoint != pCurrentContour->pFirstDataPoint )
  {
   pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint;
   count++;
  }

  Contour* pDeleteContour = pCurrentContour;  
  pCurrentContour = pCurrentContour->pNextContour;
  if( count < 2 )
  {
   delete pDeleteContour;
  }
  
 }

 return true;
}

bool Contour::RemoveBumps( void )
{
 return true; 
}

double Contour::GetClosestDistance( double x, double y )
{
 extern double dx;

 DataPoint* pClosestDataPoint = FindClosestContourDataPoint(x,y); 

 Matrix Spoints(1,5);
 Matrix Xpoints(1,5);
 Matrix Ypoints(1,5);
 
 *Spoints(2) = 0;
 *Xpoints(2) = pClosestDataPoint->x;
 *Ypoints(2) = pClosestDataPoint->y;
 
 int i = 3;
 double s = 0.0;
 
 DataPoint* pCurrentDataPoint = pClosestDataPoint->pNextDataPoint;
 int TempCount = 0;
 while( i < 5 && pCurrentDataPoint )
 {
  *Xpoints(i) = pCurrentDataPoint->x;
  *Ypoints(i) = pCurrentDataPoint->y;
  double ds = sqrt( square(*Xpoints(i)-*Xpoints(i-1)) + square(*Ypoints(i)-*Ypoints(i-1)) );
  if( ds > 0.25*dx || TempCount > 0 )
  { *Spoints(i) = *Spoints(i-1)+ds; i++; TempCount = 0; }
  else
  { TempCount++; }
  pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint;
 }
 
 while( i < 5 )
 {
  *Spoints(i) = *Spoints(i-1);
  *Xpoints(i) = *Xpoints(i-1);
  *Ypoints(i) = *Ypoints(i-1); 
  i++;
 }
 
 i = 1;
 s = 0.0;
 
 pCurrentDataPoint = pClosestDataPoint->pPreviousDataPoint;
 TempCount = 0;
 while( i > -1 && pCurrentDataPoint )
 {
  *Xpoints(i) = pCurrentDataPoint->x;
  *Ypoints(i) = pCurrentDataPoint->y;
  double ds = sqrt( square(*Xpoints(i)-*Xpoints(i+1)) + square(*Ypoints(i)-*Ypoints(i+1)) );
  if( ds > 0.25*dx || TempCount > 0 )
  { *Spoints(i) = *Spoints(i+1)-ds; i--; TempCount = 0; }
  else 
  { TempCount++; }
  pCurrentDataPoint = pCurrentDataPoint->pPreviousDataPoint;
 }
 
 while( i > -1 )
 {
  *Spoints(i) = *Spoints(i+1);
  *Xpoints(i) = *Xpoints(i+1);
  *Ypoints(i) = *Ypoints(i+1); 
  i--;
 }
 
 Polynomial Xcurve = LeastSquaresPolynomialFit(Spoints,Xpoints,2);
 Polynomial Ycurve = LeastSquaresPolynomialFit(Spoints,Ypoints,2);
 
 Xcurve.SetCoefficient( 0,*Xpoints(2) ); 
 Ycurve.SetCoefficient( 0,*Ypoints(2) ); 

 double PreviousDistance = 9e99;
 double Distance = 0;
 
 s = 0;
 int Direction = 1;
 double ds = 0.1;
 
 int MaxCount = 10000;
 int count = 0; 
 
 while( ds > 1e-9 & count < MaxCount )
 {
  Distance = sqrt( square( Xcurve(s)-x) + square( Ycurve(s)-y) );
  if( Distance > PreviousDistance )
  { ds *= 0.5; Direction *= -1; PreviousDistance = 9e99; }
  
  PreviousDistance = Distance;
  s = s + Direction*ds;
  count++;
 }
 
 return Distance;
 
 return sqrt( square( Xcurve(0)-x) + square( Ycurve(0)-y) );
} 

GeometricDataPacket Contour::GetGeometricData( double x, double y )
{
 extern double dx;
 extern double _EPS;
 extern double _PI;
 
 GeometricDataPacket Output;

 DataPoint* pClosestDataPoint = FindClosestContourDataPoint(x,y); 

 Matrix Spoints(1,5);
 Matrix Xpoints(1,5);
 Matrix Ypoints(1,5);
 
 *Spoints(2) = 0;
 *Xpoints(2) = pClosestDataPoint->x;
 *Ypoints(2) = pClosestDataPoint->y;
 
 int i = 3;
 double s = 0.0;
 
 DataPoint* pCurrentDataPoint = pClosestDataPoint->pNextDataPoint;
 int TempCount = 0;
 while( i < 5 && pCurrentDataPoint )
 {
  *Xpoints(i) = pCurrentDataPoint->x;
  *Ypoints(i) = pCurrentDataPoint->y;
  double ds = sqrt( square(*Xpoints(i)-*Xpoints(i-1)) + square(*Ypoints(i)-*Ypoints(i-1)) );
  if( ds > 0.25*dx || TempCount )
  { *Spoints(i) = *Spoints(i-1)+ds; i++; TempCount = 0; }
  else
  { TempCount++; }
  pCurrentDataPoint = pCurrentDataPoint->pNextDataPoint;
 }
 while( i < 5 )
 {
  *Spoints(i) = *Spoints(i-1);
  *Xpoints(i) = *Xpoints(i-1);
  *Ypoints(i) = *Ypoints(i-1);
  i++;  
 }
 
 i = 1;
 s = 0.0;
 
 pCurrentDataPoint = pClosestDataPoint->pPreviousDataPoint;
 TempCount = 0;
 while( i > -1 && pCurrentDataPoint )
 {
  *Xpoints(i) = pCurrentDataPoint->x;
  *Ypoints(i) = pCurrentDataPoint->y;
  double ds = sqrt( square(*Xpoints(i)-*Xpoints(i+1)) + square(*Ypoints(i)-*Ypoints(i+1)) );
  if( ds > 0.25*dx || TempCount )
  { *Spoints(i) = *Spoints(i+1)-ds; i--; TempCount = 0; }
  else
  { TempCount++; }
  pCurrentDataPoint = pCurrentDataPoint->pPreviousDataPoint;
 }
 while( i > -1 )
 {
  *Spoints(i) = *Spoints(i+1);
  *Xpoints(i) = *Xpoints(i+1);
  *Ypoints(i) = *Ypoints(i+1);
  i--;  
 }
 
 int PolyDegree = 3;
 
 Polynomial Xcurve = LeastSquaresPolynomialFit(Spoints,Xpoints,PolyDegree);
 Polynomial Ycurve = LeastSquaresPolynomialFit(Spoints,Ypoints,PolyDegree);
 
 Xcurve.SetCoefficient( 0,*Xpoints(2) ); 
 Ycurve.SetCoefficient( 0,*Ypoints(2) ); 
 
 double PreviousDistance = 9e99;
 double Distance = 0;
 
 s = 0;
 int Direction = 1;
 double ds = 0.1;
 
 int MaxCount = 10000;
 int count = 0; 
 
 while( ds > 1e-9 & count < MaxCount )
 {
  Distance = sqrt( square( Xcurve(s)-x) + square( Ycurve(s)-y) );
  if( Distance > PreviousDistance )
  { ds *= 0.5; Direction *= -1; }
  
  PreviousDistance = Distance;
  s = s + Direction*ds;
  count++;
 }
 
 Output.DistanceToInterface = Distance;

 Polynomial Xprime = Derivative( Xcurve );
 Polynomial Xdouble_prime = Derivative( Xprime );
 Polynomial Yprime = Derivative( Ycurve );
 Polynomial Ydouble_prime = Derivative( Yprime );
 
 double xp = Xprime(s);
 double xp2 = square(xp);
 double xpp = Xdouble_prime(s);
 double yp = Yprime(s);
 double yp2 = square(yp);
 double ypp = Ydouble_prime(s);
 double norm = sqrt( xp2 + yp2 + _EPS );
 
 Output.Curvature = ( xp*ypp - xpp*yp )/IntPow( norm , 3); 
 if( Output.Curvature > 1/dx )
 { Output.Curvature = 1/dx; }
 if( Output.Curvature < -1/dx )
 { Output.Curvature = -1/dx; }
 
 Output.NormalX =  yp / norm;
 Output.NormalY = -xp / norm;
 
 if( norm < 1e-7 )
 {
  double Angle = 2.0*_PI*UniformRandom();
  Output.NormalX = cos( Angle );
  Output.NormalY = sin( Angle );
 }

 return Output;
} 
